#include <iostream>
#include <vector>
#include <string>
#include <fstream>

#include <cv.h>
#include <highgui.h>

#include "../Common/Camera/PGRCamera.h"
#include "../Common/Utils/utils.h"
#include "../Common/Utils/Logger.h"
#include "../Common/Calibration/Calibration.h"
#include "../Common/Calibration/Calibrator.h"

int WIDTH = 320;
int HEIGHT = (WIDTH * 3) / 4;
const int CALIBRATION_PATTERN_COUNT = 30;
const int CALIBRATION_WIDTH_COUNT = 8;
const int CALIBRATION_MINIMUM_COUNT = 4;

const int S_WIDTH = WIDTH/5;
const int S_HEIGHT = HEIGHT/5;

const char* SAVE_FILE_NAME = "Calibration_Save_Images/image%02d.jpg";
const char* SAVE_INTRINSIC_NAME = "Calibration_Save_Images/intrinsic";
const char* SAVE_EXTRINSIC_NAME = "Calibration_Save_Images/extrinsic";

const int CHESSBOARD_WIDTH_COUNT = 7;
const int CHESSBOARD_HEIGHT_COUNT = 8;
const int CHESSBOARD_POINT_COUNT = (CHESSBOARD_WIDTH_COUNT-1) * (CHESSBOARD_HEIGHT_COUNT-1);
const double CHESSBOARD_FIELD_SIZE = 28.0; // mm

void main()
{
	int useCamera = 0;
	std::cout << "Use the camera?(true=1, false=0) : ";
	std::cin >> useCamera;
	std::cout << "Enter the camera image size (640) : ";
	std::cin >> WIDTH;
	HEIGHT = (WIDTH * 3) / 4;
	std::cout << "camera width : " << WIDTH << ", height : " << HEIGHT << std::endl;

	// camera
	CPGRCamera* camera = NULL;
	if(useCamera == 1)
	{
		cvNamedWindow("camera");
		camera = new CPGRCamera();
		camera->open();
		camera->start();
	}
	else
	{
		std::cout << "Press 'l' to load pattern images" << std::endl;
		std::cout << "Press 'r' to reset pattern images" << std::endl;
		std::cout << "Press 's' to solve calibration" << std::endl;
		std::cout << "Press 'q' to terminate program" << std::endl;
	}
	cvNamedWindow("saved images");
	
	// chessboard tracker
	windage::Calibration* calibration = new windage::Calibration();
	calibration->SetSize(WIDTH, HEIGHT);

	windage::Calibrator* calibrator = new windage::Calibrator();
	calibrator->Initialize(CHESSBOARD_WIDTH_COUNT, CHESSBOARD_HEIGHT_COUNT, CHESSBOARD_FIELD_SIZE);
	calibrator->AttatchCalibration(calibration);

	// images
	IplImage* resultImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);
	IplImage* grayImage = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
	IplImage* savedImage = cvCreateImage(cvSize(S_WIDTH * CALIBRATION_WIDTH_COUNT, S_HEIGHT * (CALIBRATION_PATTERN_COUNT/CALIBRATION_WIDTH_COUNT+1)), IPL_DEPTH_8U, 3);

	// save pattern image info
	int savedCount = 0;

	bool showInformation = false;
	bool isSolving = false;
	bool updateExtrinsic = false;
	bool addPatternImage = false;
	bool processing = true;
	while(processing)
	{
		char message[100];
		double startTime = cvGetTickCount();

		// grab the camera image
		if(useCamera == 1)
		{
			camera->update();
			IplImage* grabImage = camera->GetIPLImage();
			cvResize(grabImage, grayImage);
			cvCvtColor(grayImage, resultImage, CV_GRAY2BGR);

			// find chessboard
			std::vector<CvPoint2D32f> chessboardPoints;
			if(calibrator->FindChessboardCorner(&chessboardPoints, grayImage))
			{
				calibrator->DrawChessboardInfo(&chessboardPoints, resultImage);

				// save pattern image
				if(addPatternImage && savedCount < CALIBRATION_PATTERN_COUNT)
				{
					int width = savedCount % CALIBRATION_WIDTH_COUNT;
					int height = savedCount / CALIBRATION_WIDTH_COUNT;

					cvSetImageROI(savedImage, cvRect(S_WIDTH*width, S_HEIGHT*height, S_WIDTH, S_HEIGHT));
					cvResize(resultImage, savedImage);
					cvResetImageROI(savedImage);
					sprintf(message, SAVE_FILE_NAME, savedCount+1);
					cvSaveImage(message, grayImage);

					calibrator->AddChessboardPoints(chessboardPoints);
					savedCount++;
				}

				// update extrinsic params
				if(updateExtrinsic && isSolving)
				{
					calibrator->UpdateExtrinsic(&chessboardPoints);
					calibration->DrawInfomation(resultImage, 100.0);
				}
			}
			chessboardPoints.clear();

			// draw result
			double processingTime = (cvGetTickCount() - startTime)/(cvGetTickFrequency() * 1000.0);
			sprintf(message, "Prcessing time : %.2f ms",  processingTime);
			std::cout << message << std::endl;
			DrawTextToImage(resultImage, cvPoint(WIDTH - 200, HEIGHT-15), message);

			if(showInformation)
			{
				DrawTextToImage(resultImage, cvPoint(15, 15), "Press 'q' to terminate program");
				DrawTextToImage(resultImage, cvPoint(15, 30), "Press 'a' to save pattern image");
				DrawTextToImage(resultImage, cvPoint(15, 45), "Press 'l' to load pattern images");
				DrawTextToImage(resultImage, cvPoint(15, 60), "Press 'r' to reset pattern images");
				if(savedCount >= CALIBRATION_MINIMUM_COUNT)
					DrawTextToImage(resultImage, cvPoint(15, 75), "Press 's' to solve calibration");
				sprintf(message, "saved image count : %d", savedCount);
				DrawTextToImage(resultImage, cvPoint(15, 90), message);
				if(isSolving == true)
				{
					DrawTextToImage(resultImage, cvPoint(15, 105), "Press 'u' to update extrinsic params");
					DrawTextToImage(resultImage, cvPoint(15, 120), "Press 'v' to save calibration params");
				}
			}
			else
			{
				DrawTextToImage(resultImage, cvPoint(15, 15), "Press 'i' toggle to show information");
			}

			cvShowImage("camera", resultImage);
		}
		cvShowImage("saved images", savedImage);

		addPatternImage = false;
		char ch = cvWaitKey(1);
		switch(ch)
		{
		case 27:
		case 'q' : 
		case 'Q' :
			processing = false;
			break;
		case 'a' :
		case 'A' :
			addPatternImage = true;
			break;
		case 'u' :
		case 'U' :
			updateExtrinsic = !updateExtrinsic;
			break;
		case 'i' :
		case 'I' :
			showInformation = !showInformation;
			break;
		case 'l' :
		case 'L' :
			{
				// reset
				calibrator->ResetChessboardPointList();				
				cvSetZero(savedImage);
				savedCount = 0;

				for(int i=0; i<CALIBRATION_PATTERN_COUNT; i++)
				{
					sprintf(message, SAVE_FILE_NAME, i+1);
					IplImage* tempGray = cvLoadImage(message, 0);
					if(tempGray)
					{
						cvResize(tempGray, grayImage);
						cvCvtColor(grayImage, resultImage, CV_GRAY2BGR);

						std::vector<CvPoint2D32f> chessboardPoints;
						if(calibrator->FindChessboardCorner(&chessboardPoints, grayImage))
						{
							calibrator->DrawChessboardInfo(&chessboardPoints, resultImage);

							// save pattern image
							if(savedCount < CALIBRATION_PATTERN_COUNT)
							{
								int width = savedCount % CALIBRATION_WIDTH_COUNT;
								int height = savedCount / CALIBRATION_WIDTH_COUNT;

								cvSetImageROI(savedImage, cvRect(S_WIDTH*width, S_HEIGHT*height, S_WIDTH, S_HEIGHT));
								cvResize(resultImage, savedImage);
								cvResetImageROI(savedImage);
//								sprintf(message, SAVE_FILE_NAME, savedCount+1);
//								cvSaveImage(message, grayImage);

								savedCount++;
								calibrator->AddChessboardPoints(chessboardPoints);
							}
						}
						chessboardPoints.clear();

						cvReleaseImage(&tempGray);
					}
				}
			}
			break;	
		case 'r' : // reset 
		case 'R' :
			{
				calibrator->ResetChessboardPointList();				
				cvSetZero(savedImage);
				savedCount = 0;
			}
			break;
		case 's':
		case 'S':
			{
				if(savedCount >= CALIBRATION_MINIMUM_COUNT)
				{
					std::cout << std::endl;
					std::cout << "******************************"  << std::endl;
					std::cout << "solving camera calibration" << std::endl;

					calibrator->SolveCalibration();
					CvSize size = calibration->GetSize();
					CvMat* intrinsicMatrix = calibration->GetIntrinsicMatrix();
					CvMat* distortionCoefficients = calibration->GetDistortionCoefficients();
					CvMat* extrinsicMatrix = calibration->GetExtrinsicMatrix();

					std::cout << "******************************"  << std::endl;
					std::cout << "image information" << std::endl;
					std::cout << size.width << ", " << size.height << std::endl;
					std::cout << std::endl;

					std::cout << "******************************"  << std::endl;
					std::cout << "intrinsic matrix" << std::endl;
					std::cout << cvGetReal2D(intrinsicMatrix, 0, 0) << ", " << cvGetReal2D(intrinsicMatrix, 0, 1) << ", " << cvGetReal2D(intrinsicMatrix, 0, 2) << std::endl;
					std::cout << cvGetReal2D(intrinsicMatrix, 1, 0) << ", " << cvGetReal2D(intrinsicMatrix, 1, 1) << ", " << cvGetReal2D(intrinsicMatrix, 1, 2) << std::endl;
					std::cout << cvGetReal2D(intrinsicMatrix, 2, 0) << ", " << cvGetReal2D(intrinsicMatrix, 2, 1) << ", " << cvGetReal2D(intrinsicMatrix, 2, 2) << std::endl;
					std::cout << std::endl;

					std::cout << "******************************"  << std::endl;
					std::cout << "distortion coefficients" << std::endl;
					std::cout << cvGetReal1D(distortionCoefficients, 0) << " " << cvGetReal1D(distortionCoefficients, 1) << " " << cvGetReal1D(distortionCoefficients, 2) << " " << cvGetReal1D(distortionCoefficients, 3) << std::endl;
					std::cout << std::endl;

					isSolving = true;
				}
				else
				{
					std::cout << "not enough saved image to solve calibration" << std::endl;
				}
			}
			break;
		case 'v' :
		case 'V':
			{
				// save data to file
				CvSize size = calibration->GetSize();
				CvMat* intrinsicMatrix = calibration->GetIntrinsicMatrix();
				CvMat* distortionCoefficients = calibration->GetDistortionCoefficients();
				CvMat* extrinsicMatrix = calibration->GetExtrinsicMatrix();

				windage::Logger saveLogger(SAVE_INTRINSIC_NAME, true);
				saveLogger.log(size.width);
				saveLogger.log(size.height);
				saveLogger.logNewLine();
				saveLogger.log(cvGetReal2D(intrinsicMatrix, 0, 0));
				saveLogger.log(cvGetReal2D(intrinsicMatrix, 1, 1));
				saveLogger.log(cvGetReal2D(intrinsicMatrix, 0, 2));
				saveLogger.log(cvGetReal2D(intrinsicMatrix, 1, 2));
				saveLogger.logNewLine();
				saveLogger.log(cvGetReal1D(distortionCoefficients, 0));
				saveLogger.log(cvGetReal1D(distortionCoefficients, 1));
				saveLogger.log(cvGetReal1D(distortionCoefficients, 2));
				saveLogger.log(cvGetReal1D(distortionCoefficients, 3));
				saveLogger.logNewLine();

				windage::Logger saveExtrinsic(SAVE_EXTRINSIC_NAME, true);
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 0, 0));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 0, 1));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 0, 2));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 0, 3));
				saveExtrinsic.logNewLine();
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 1, 0));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 1, 1));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 1, 2));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 1, 3));
				saveExtrinsic.logNewLine();
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 2, 0));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 2, 1));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 2, 2));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 2, 3));
				saveExtrinsic.logNewLine();
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 3, 0));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 3, 1));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 3, 2));
				saveExtrinsic.log(cvGetReal2D(extrinsicMatrix, 3, 3));
				saveExtrinsic.logNewLine();
			}
			break;
		}
	}

	if(useCamera == 1)
	{
		camera->stop();
		camera->close();
		delete camera;
	}

	cvDestroyAllWindows();
}